//initialisatie functies
void sendstartschot(); // startschot versturen
//int receiveIDs(); //IDs van auto's ontvangen
int IDtoekenen(int maxid);
//void givedistance(int maxid); //zeggen dat ze data moeten sturen

//main functies
void drive(unsigned char id); // auto's laten rijden
int check(int maxid); //data controleren
int inlezen();
void send(int length);
void progressIDs(int inimessages);
void extractdistance(int nrmessages, int driven, int pogingnr);
int afstandeninlezen();

void char2bin(unsigned char number, int bitarray[8]); 
unsigned char bin2char(int bitarray[8]); 
void encode(unsigned char number, int bitarray1[8], int bitarray2[8]);
int decode(unsigned char * number, int * bitarray1, int * bitarray2);

//globale variabelen
unsigned char codeddata[64]; //data die we binnenkrijgen
unsigned char data[20]; //data die we binnenkrijgen die gedecodeerd is
unsigned char newdata[32]; //data die we gaan versturen
unsigned char codedsend[64];//gedecodeerde data die we gaan versturen
unsigned char afstanden[10];
unsigned char IDs[10][8];//lange id's
unsigned char afstandmssg[10][12];
unsigned char decodeafstand[10][6];
unsigned char aardevector[10];
unsigned char fullids[100][24];
unsigned char allids[100][8];

int maxid = 0;
int maxID = 0;
int sendmessagelength = 10;
char *rs232_port = (char*)0xC00000;//set memory

void main()
{
	int test=0;
	char a[2]= {1,2};
	char * point = &a[0];
	unsigned char iets =0;
	unsigned char * pnt = &iets;
	int bit1[8]={0,0,0,0,0,0,0,0};
	int * b1 = &bit1[0];
	int bit2[8]={0,0,0,0,0,0,0,0};
	int * b2 = &bit2[0];
	int nrmssg=0;


	sendstartschot();
	maxID = IDtoekenen(maxid);
	inlezen();
	send(4);
	test = check(6);
	drive(3);
	test= decode(pnt, b1, b2) ;
	nrmssg =  afstandeninlezen();
	extractdistance(nrmssg, 3, 1);
	
}


void sendstartschot()
{
	newdata[0] = 16;//dle
	newdata[1] = 19;//start bug
    newdata[2] = 33;// opcode 33 voor initialisatie
	newdata[3] = 16;//dle
	newdata[4] = 20;//stop bug
    newdata[5] = '\r';
    newdata[6] = '\n';
	send(7);
}

void char2bin(unsigned char number, int bitarray[8]) 
{
    int j;
    for (j=0; j<8; j++) 
	{
        bitarray[j] = ((number >> j) && 1);
    }
}

unsigned char bin2char(int bitarray[8]) 
{
    unsigned char number = 0;
    int i;

    for(i=0; i<8; i++) 
	{
        if(bitarray[i]!=0) 
		{
            number += 1<<i;
        }
    }
    return number;
}

int check(int maxid)
{

    char errors = 0;
    char i = 0;
    for(i = 0; i < maxID; i++)
    {
        if(data[i] < 20)
        {
            data[i] = 255;
            errors = errors + 1;
        }
        else if(data[i] > 200)
        {
            data[i] = 255;
            errors = errors +1;
        }
    }
    if(errors > maxid-3)
    {
        return 0;
    }
    else
    {
        return 1;
    }
}

void drive(unsigned char id)
{
    newdata[0]=16;//dle
	newdata[1]=19;//start bug
	newdata[2]=11;//opcode voor rijden
	newdata[3]=id;//id auto
	newdata[4]=16;//dle
	newdata[5]=20;//stop bug
	newdata[6]='\r';
	newdata[7]='\n';
	send(8);
}

void encode(unsigned char number, int bitarray1[8], int bitarray2[8]) 
{
    int bitarray[8];
	//b3, b3^b2^b1, b2, !b2^b1^b0, b1, !b3^b1^b0, b0, !b3^b2^b0
    
    char2bin(number, bitarray);

    bitarray1[7] = (bitarray[3]);
    bitarray1[6] = (bitarray[3])^(bitarray[2])^(bitarray[1]);
    bitarray1[5] = (bitarray[2]);
    bitarray1[4] = (!bitarray[2])^(bitarray[1])^(bitarray[0]);
    bitarray1[3] = (bitarray[1]);
    bitarray1[2] = (!bitarray[3])^(bitarray[1])^(bitarray[0]);
    bitarray1[1] = (bitarray[0]);
    bitarray1[0] = (!bitarray[3])^(bitarray[2])^(bitarray[0]);

    bitarray2[7] = (bitarray[7]);
    bitarray2[6] = (bitarray[7])^(bitarray[6])^(bitarray[5]);
    bitarray2[5] = (bitarray[6]);
    bitarray2[4] = (!bitarray[6])^(bitarray[5])^(bitarray[4]);
    bitarray2[3] = (bitarray[5]);
    bitarray2[2] = (!bitarray[7])^(bitarray[5])^(bitarray[4]);
    bitarray2[1] = (bitarray[4]);
    bitarray2[0] = (!bitarray[7])^(bitarray[6])^(bitarray[4]);
}

int decode(unsigned char * number, int * bitarray1, int * bitarray2) {
    //    p  = h7 ^ h6 ^ h5 ^ h4 ^ h3 ^ h2 ^ h1 ^ h0
    //    c0 = h7 ^ h5 ^ h1 ^ h0
    //    c1 = h7 ^ h3 ^ h2 ^ h1
    //    c2 = h5 ^ h4 ^ h3 ^ h1
    int bitarray[8];
    int p1, p2, c01, c02, c11, c12, c21, c22;
    
    p1  = (bitarray1[7]) ^ (bitarray1[6]) ^ (bitarray1[5]) ^ (bitarray1[4]) ^ (bitarray1[3]) ^ (bitarray1[2]) ^ (bitarray1[1]) ^ (bitarray1[0]);
    c01 = (bitarray1[7]) ^ (bitarray1[5]) ^ (bitarray1[1]) ^ (bitarray1[0]);
    c11 = (bitarray1[7]) ^ (bitarray1[3]) ^ (bitarray1[2]) ^ (bitarray1[1]);
    c21 = (bitarray1[5]) ^ (bitarray1[4]) ^ (bitarray1[3]) ^ (bitarray1[1]);

    p2  = (bitarray2[7]) ^ (bitarray2[6]) ^ (bitarray2[5]) ^ (bitarray2[4]) ^ (bitarray2[3]) ^ (bitarray2[2]) ^ (bitarray2[1]) ^ (bitarray2[0]);
    c02 = (bitarray2[7]) ^ (bitarray2[5]) ^ (bitarray2[1]) ^ (bitarray2[0]);
    c12 = (bitarray2[7]) ^ (bitarray2[3]) ^ (bitarray2[2]) ^ (bitarray2[1]);
    c22 = (bitarray2[5]) ^ (bitarray2[4]) ^ (bitarray2[3]) ^ (bitarray2[1]);

    // If the parity, p, is correct (equal to 1) then either 0 or 2 errors occurred. If all the check bits, c0, c1, c2 are correct (equal to 1) then the byte was received intact, (no errors) otherwise it was damaged beyond repair (two errors).
    if((p1 && !(c01 && c11 && c21)) || (p2 && !(c02 && c12 && c22))) {
        return 0;
    }

    // h7, h5, h3, h1
    bitarray[7] = (bitarray2[7]);
    bitarray[6] = (bitarray2[5]);
    bitarray[5] = (bitarray2[3]);
    bitarray[4] = (bitarray2[1]);
    bitarray[3] = (bitarray1[7]);
    bitarray[2] = (bitarray1[5]);
    bitarray[1] = (bitarray1[3]);
    bitarray[0] = (bitarray1[1]);

    //     c0 c1 c2    meaning
    //     0  0  1    error in bit h7
    //     0  1  0    error in bit h5
    //     1  0  0    error in bit h3
    //     0  0  0    error in bit h1
    if(!p2) {
        if(!c02 && !c12 && c22)
            bitarray[7] = !(bitarray[7]);
        if(!c02 && c12 && !c22)
            bitarray[6] = !(bitarray[6]);
        if(c02 && !c12 && !c22)
            bitarray[5] = !(bitarray[5]);
        if(!c02 && !c12 && !c22)
            bitarray[4] = !(bitarray[4]);
    }
    if(!p1) {
        if(!c01 && !c11 && c21)
            bitarray[3] = !(bitarray[3]);
        if(!c01 && c11 && !c21)
            bitarray[2] = !(bitarray[2]);
        if(c01 && !c11 && !c21)
            bitarray[1] = !(bitarray[1]);
        if(!c01 && !c11 && !c21)
            bitarray[0] = !(bitarray[0]);
    }

    *number = bin2char(bitarray);
    return 1;
}

void send(int length)     
{
	int mssgnr=0;
	int codenr=0;
	int skip=0;
	int dle=0;
	int bit1[8];
	int bit2[8];
	int i=0;
	int lengte=0;
	while(mssgnr<length)
	{		
		skip=codenr*2;
		encode(data[mssgnr], bit1, bit2);
		codedsend[skip] = bin2char(bit1);
		codedsend[skip+1] = bin2char(bit2);
		if(newdata[mssgnr]==16 && dle==0)
		{
			dle=1;
			i--;
		}
		else if(dle==1)
		{
			dle=0;
		}
		mssgnr++;
		codenr++;
	}
	lengte=codenr*2;
	while(i<lengte)
	{
		rs232_port[0] = codedsend[i];
	}
}

int inlezen(int length) //length is zonder hamming
{
	int i=0;
	int j=0;
	int skip=0;
	int returnvalue=0;
	int hamming=2*length;
	unsigned char chars[32];
	int bit1[8];
	int bit2[8];
	int timing=0;
	codeddata[0]=2;
	while(1)
	{
		codeddata[0]=rs232_port[0];
		if(codeddata[0]==69)
		{
			timing++;
		}
		else if(codeddata[0]!=69)
		{
			break;
		}
		if(timing>3)
		{
			return 1;
		}
	}
	i++;
	while(i<hamming)
	{
		codeddata[i]=rs232_port[0];
		if(codeddata[i-1]== 21 && codeddata[i]==2) 
		{
			i--;
			i--;
		}
		else if(
		i++;
	}
	
	i=0;
	while(i<length)
	{
		skip=length*2;
		char2bin(codeddata[skip], bit1);
		char2bin(codeddata[skip+1], bit2);
		//returnvalue = decode(data[i], bit1, bit2);!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		i++;
	}
	return 0;
	
}

int IDtoekenen(int maxid) 
{
	int i=0;
	int j=0;
	int b=0;
	int carnumber=0;
	int skip=0;
	unsigned char input=0;
	newdata[0] = 16;//dle
	newdata[1] = 19;//start bug
    newdata[2] = 33;//opcode 44 voor toekenen id
	//lange id en nieuwe id
	newdata[12] = 16;//dle
	newdata[13] = 20;//stop bug
    newdata[14] = '\r';
    newdata[15] = '\n';
	for(i=0;i<maxID;i++)
	{
		for(j=0;j<8;j++)
		{
			newdata[j+3]=IDs[i][j];
		}
		newdata[11]=carnumber;
		send(16);
		b=0;
		while(1)
		{
			input = rs232_port[0];

			if(input == 100)
			{
				carnumber++;
				break;
			}
			else if(input == 69)
			{
				b++;
			}
			if(b == 5)
			{
				break;
			}	
		}
	}
	return carnumber;
}

int afstandeninlezen()
{
	int i=0;
	int j=0;
	int skip=0;
	int returnvalue=0;
	int hamming=2*length;
	unsigned char chars[32];
	int bit1[8];
	int bit2[8];
	int timing=0;
	afstandmssg[0]=2;
	int mssgnr=0;

	while(timing<5)
	{
		while(1)
		{
			afstandmssg[mssgnr][0]=rs232_port[0];
			if(afstandmssg[mssgnr][0]==69 ||afstandmssg[mssgnr][0]==100)
			{
				timing++;
			}
			else
			{
				break;
			}
		}
		i++;
		while(i<12)
		{
			afstandmssg[mssgnr][i]=rs232_port[0];
			if(afstandmssg[mssgnr][i-1]== 21 && afstandmssg[mssgnr][i]==2) 
			{
				i--;
				i--;
			}
			i++;
		}
		mssgnr++;
		i=0;
	}
	i=0;
	int j=0;
	for(j=0;j<mssgnr;j++)
	{
		while(i<length)
		{
			skip=length*2;
			char2bin(afstandmssg[j][skip], bit1);
			char2bin(afstandmssg[[j]skip+1], bit2);
			//returnvalue = decode(decodeafstand[j][i], bit1, bit2);!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			i++;
		}
		return mssgnr;
	}
}

int extractdistance(int nrmessages, int driven, int pogingnr)
{
	int i=0;
	int succes=0;

	for(i;i<nrmessages;i++)
	{
		aardevector[decodeafstand[i][3]]=decodeafstand[i][4];
	}
	aardevector[driven]=0;
	succes = check(maxID);
	if(succes)
		return 0;
	else
		pogingnr++;

	if(pogingnr>3)
		return -1;
	else
		return pogingnr;
}


void sendaarde()     
{
	int mssgnr=0;
	int codenr=0;
	int skip=0;
	int dle=0;
	int bit1[8];
	int bit2[8];
	int i=0;
	int lengte=0;
	int length=0;
	for(i;i<maxID;i++)
	{
		newdata[i+2]=aardevector[i];
	}
	i=0;
	length=maxID+4;
	while(mssgnr<length)
	{		
		skip=codenr*2;
		encode(data[mssgnr], bit1, bit2);
		codedsend[skip] = bin2char(bit1);
		codedsend[skip+1] = bin2char(bit2);
		if(newdata[mssgnr]==16 && dle==0)
		{
			dle=1;
			mssgnr--;
		}
		else if(dle==1)
		{
			dle=0;
		}
		mssgnr++;
		codenr++;
	}
	lengte=codenr*2;

	//dle start a0 a1 a2...amaxID dle stop =maxID+4
	newdata[0] = 16;//dle
	newdata[1] = 17;//start aarde
	//afstanden (maxID groot)
	newdata[maxID+3] = 16;//dle
	newdata[maxID+4] = 18;//stop aarde

	while(i<lengte)
	{
		rs232_port[0] = codedsend[i];
	}
}

void progressIDs(int inimessages)
{
	IDs[10][8]
    int mssg = 1;
    int t = inimessages-1;
	int i=0;
	int j=0;
	int k=0;
	int nieuw=0;
	int ids=1;
	for(i=0;i<8;i++)
	{
		IDs[0][i]=allids[0][i];
	}
	for(mssg; mssg<t; mssg++)
    {
		nieuw=0;
		for(j=0;j<ids;j++)
		{
			for(i = 0; i<8; i++)
			{
				if(allids[mssg][i]==IDs[j][i])
					continue;
				else
				{
					nieuw++;
					break;
				}
			}
			if(nieuw==0)
			{
				break;
			}	
		}
		if(nieuw==ids)
		{
			for(k=0;k<8;k++)
			{
				IDs[ids][k]=allids[mssg][k];
			}
			ids++;
		}
    }
}



int receiveIDs()
{
    int mssgnr=0;
    int timing= 0;
    int i = 0;

    for(mssgnr = 0; mssgnr<100; mssgnr++)
    {
		fullids[mssgnr][0] = rs232_port[0];
		if(fullids[mssgnr][0] == ((char) 69))
            {
                timing++;
            }
            if(timing > 42)
            {
                return mssgnr;
            }
        for(i = 1; i<24; i++)
        {
            fullids[mssgnr][i] = rs232_port[0];
			if(afstandmssg[mssgnr][i-1]== 21 && afstandmssg[mssgnr][i]==2) 
			{
				i--;
				i--;
			}
        }
    }
	for(j=0;j<mssgnr;j++)
	{
		while(i<8)
		{
			skip=length*2;
			char2bin(afstandmssg[j][skip+4], bit1);
			char2bin(afstandmssg[[j]skip+5], bit2);
			//returnvalue = decode(allids[j][i], bit1, bit2);!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			i++;
		}
	}
}

